/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;

import file.Configurable;
import file.Configuration;
import file.DataRecorder;

/**
 *
 * @author Mentors  Carl Burger - Team 708
 */
public class Wrist implements Configurable {

    // Sensors
    private  AnalogChannel  wristSensor;   // potentiometer for wrist position
    private  PIDSourceProxy wristPotProxy; // overrides pidGet()

    // Actuators
    private  Victor         wristMotor;


    private  DataRecorder   dataRecorder;
    private  MessageCenter  messageCenter;

    private  int previousWristPos = 0;       // for message center display
    private  double previousWristOutput = -2; //make sure that value is output first time around

    private  Timer  timer;
    private  double wristTimeLimit = 4;    // max secs to reach setpoint

    private  double lastManualMovement = 0.0;  // last input from joystick (0 if in dead zone)

    private  PIDController708 wristControl;

    private  double minimumInput  = 315;    // potentiometer minimum averageValue; ~1.6v
    private  double maximumInput  = 858;    // potentiometer maximum averageValue; ~4.4v
    private  double tolerance     = 1.0;    // 1% of (max - min)
    private  double minimumOutput = -1.0;   // motor minimum speed
    private  double maximumOutput = 1.0;    // motor maximum speed
    private  double proportional  = 0.005;  // needs to be tuned
    private  double integral      = 0.001;  // needs to be tuned
    private  double differential  = 0.0;    // needs to be tuned

    private  double initialPos       = 574; // wrist position at start of match
    private  double drivePos         = 1.0;
    private  double feederPos        = 574; // ~2.93v
    private  double floorPos         = 333; // ~1.7v
    private  double pegPosFromFeeder = 808; // ~4.15v
    private  double pegPosFromFloor  = 524; // ~2.7v
    private  double releaseOffset    = 0;
    private  double hangOffset       = 0;
    private  double pickupPos        = feederPos; // initial value

    private boolean joystickControl        = false;

    private  double manualSpeedGain = 2.0; // see constructor for use
    private  double manualSpeed;           // computed using  gain * tolerance
    private  double wristDeadZone   = 0.2; //absolute value of range around 0 in which JS input is ignored

    private static Wrist         m_instance = null;


    /**
     *  Get an instance of the Wrist
     */
    public static Wrist getInstance() {

        if (m_instance == null) {

            m_instance = new Wrist();

            Configuration.getInstance().register("Wrist", m_instance);  // register here to avoid leaking constructor warning
        }

        return m_instance;
    }


    public Wrist() {

        wristSensor  = new AnalogChannel(LogomotionRobot.kWrist);
//        wristPotProxy = new PIDSourceProxy(wristSensor);

        wristMotor   = new Victor(LogomotionRobot.kWristController);

//        wristMotor.setExpiration(5);
//        wristMotor.setSafetyEnabled(true);

        // initialize the PIDController which controls wrist movement

        wristControl = new PIDController708(proportional, integral, differential,
                                            wristSensor, wristMotor);
        wristControl.setInputRange(minimumInput, maximumInput);
        wristControl.setTolerance(tolerance);
        wristControl.setOutputRange(minimumOutput, maximumOutput);

        // ensure the manual speed delta exceeds the tolerance so that the
        // PIDController is guaranteed to move the wrist
        manualSpeed   = manualSpeedGain * ((maximumInput - minimumInput) * tolerance) / 100;

        timer         = new Timer();

        dataRecorder  = DataRecorder.getInstance();
        messageCenter = MessageCenter.getInstance();
    }

    
    public void monitorMovement() {
        // if the wrist is moving, check if it reached the commanded posiiton

        if (wristControl.isEnable()) {
            if (wristControl.onTarget() || timer.get() > wristTimeLimit) {

                // stop running the PIDController. Sets output to 0 before stopping
                wristControl.reset();

                timer.stop();
            }
        }
    }

    public boolean onTarget(){
        return wristControl.onTarget();
    }

    private void moveWristTo(double pos) {

        // position the wrist to commanded position
        wristControl.setSetpoint(pos);

        System.out.println("Wrist setpoint: " + pos + " average value: " + wristSensor.getAverageValue()
                + " PID " + proportional + " " + integral + " " + differential);



        if (wristControl.onTarget() == false) {
            if (wristControl.isEnable() == false) {
                
                wristControl.reset();  // clear previous errors
                wristControl.enable(); // start calculating PIDOutput values

                timer.reset();
                timer.start();
            }
        }
    }

    public void hang() {

        // position the wrist to hang a shape on a peg

        if (pickupPos == floorPos) {

            moveWristTo(pegPosFromFloor);
            System.out.println("Wrist peg pos from floor");
        }
        else {
            
            moveWristTo(pegPosFromFeeder);
            System.out.println("Wrist peg pos from feeder");
        }
    }

    public void feed() {

        // position the wrist to receive a shape from the feeder
        moveWristTo(feederPos);
        pickupPos = feederPos;
    }

    public boolean fromFloor() {

        return (pickupPos == floorPos) ? true : false;
    }

    public void pickup() {

        // position the wrist to pick up a shape from the floor
        moveWristTo(floorPos);
        pickupPos = floorPos;
    }

    public void lower() {

        double newPos;

        // allow the wrist to be manually lowered by the operator

        newPos = wristSensor.pidGet() - manualSpeed;
        if (newPos >= minimumInput) {
            moveWristTo(newPos);
        }
    }

    public void raise() {

        double newPos;

        // allow the wrist to be manually raised by the operator

        newPos = wristSensor.pidGet() + manualSpeed;
        if (newPos <= maximumInput) {
            moveWristTo(newPos);
        }
    }

    // move the wrist to release the shape on the peg
    public void release() {
        if (pickupPos == floorPos) {

            moveWristTo(pegPosFromFloor - releaseOffset);
        }
        else {

            moveWristTo(wristSensor.getAverageValue() - releaseOffset);

//            moveWristTo(pegPosFromFeeder - releaseOffset);
        }
        
    }

    public void flick()
    {
        moveWristTo(wristSensor.getAverageValue() - hangOffset);
//        moveWristTo(feederPos);
    }

    public void move(double jsInput) {

        //do not use joystick if PID control is running
        if (wristControl.isEnable() == false) {
        
            // allow the wrist to be manually moved using a joystick for debugging

            double input = jsInput;

            if ((jsInput > -wristDeadZone) && (jsInput < wristDeadZone)) {
                input = 0.0; // ignore possible joystick jitter around 0
            }

            if (input < 0) {
                input = 0.5 * input;  // slow 'er down
                if (wristSensor.getAverageValue() < minimumInput) {
                    input = 0.0;  // prevent wrist from going down too far
                }
            } else if (input > 0) {
                if (wristSensor.getAverageValue() > 800) {
                    input = 0.5 * input;  // prevent wrist from overshooting
                }
                if (wristSensor.getAverageValue() > maximumInput) {
                    input = 0.0;  // prevent wrist from going up too far
                }
            }

            if (input != lastManualMovement) {

//                System.out.println("Wrist move jsinput " + messageCenter.formatDouble(jsInput, 6) +
//                        " input " + messageCenter.formatDouble(input, 6) +
//                        " voltage: " + messageCenter.formatDouble(wristSensor.getVoltage(), 6) +
//                        " avgValue: " + wristSensor.getAverageValue());

                lastManualMovement = input;
            }

            wristMotor.set(input);

        }
    }

    public void outputStatus() {

        int wristPos = wristSensor.getAverageValue();

        if (wristPos != previousWristPos) {

            messageCenter.println(MessageCenter.kSensorsScreen, MessageCenter.kLine4,
                                  "Wrist: " + messageCenter.formatDouble(wristSensor.getVoltage(), 5) +
                                  "v " + wristPos);

            previousWristPos = wristPos;
        }

        if(wristMotor.get() != previousWristOutput){
            //display motor output
            messageCenter.println(MessageCenter.kSensorsScreen,
                        MessageCenter.kLine5,
                        "MotorOut: " + DataRecorder.formatDouble(wristMotor.get(),2));

            previousWristOutput = wristMotor.get();
        }
    }

    public void recordStatus(){
        dataRecorder.record("wrist pos",         wristSensor.pidGet());
        dataRecorder.record("wrist pos voltage", wristSensor.getVoltage());
        dataRecorder.record("wrist setpoint",    wristControl.getSetpoint());
        dataRecorder.record("wrist error",       wristControl.getError());
        dataRecorder.record("wrist P coeff",     wristControl.getP());
        dataRecorder.record("wrist I coeff",     wristControl.getI());
        dataRecorder.record("wrist D coeff",     wristControl.getD());
        dataRecorder.record("wrist PID result",  wristControl.get());
    }

    public void update(String name, String[] values) {
        if (name.equals("Minimum.Input")) {
            minimumInput = Double.parseDouble(values[0]);
            wristControl.setInputRange(minimumInput, maximumInput);

        } else if (name.equals("Maximum.Input")) {
            maximumInput = Double.parseDouble(values[0]);
            wristControl.setInputRange(minimumInput, maximumInput);

        } else if (name.equals("Tolerance")) {
            tolerance = Double.parseDouble(values[0]);
            wristControl.setTolerance(tolerance);

        } else if (name.equals("Mimimum.Output")) {
            minimumOutput = Double.parseDouble(values[0]);
            wristControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Maximum.Output")) {
            maximumOutput = Double.parseDouble(values[0]);
            wristControl.setOutputRange(minimumOutput, maximumOutput);

        } else if (name.equals("Proportional")) {
            proportional = Double.parseDouble(values[0]);
            wristControl.setPID(proportional,integral,differential);

        } else if (name.equals("Integral")) {
            integral = Double.parseDouble(values[0]);
            wristControl.setPID(proportional,integral,differential);

        } else if (name.equals("Differential")) {
            differential = Double.parseDouble(values[0]);
            wristControl.setPID(proportional,integral,differential);

        } else if (name.equals("Initial.Pos")) {
            initialPos = Double.parseDouble(values[0]);

        } else if (name.equals("Drive.Pos")) {
            drivePos = Double.parseDouble(values[0]);

        } else if (name.equals("Feeder.Pos")) {
            feederPos = Double.parseDouble(values[0]);

        } else if (name.equals("Floor.Pos")) {
            floorPos = Double.parseDouble(values[0]);

        } else if (name.equals("Peg.Pos.From.Floor")) {
            pegPosFromFloor = Double.parseDouble(values[0]);

        } else if (name.equals("Peg.Pos.From.Feeder")) {
            pegPosFromFeeder = Double.parseDouble(values[0]);

        }else if(name.equals("Release.Offset")){
            releaseOffset = Double.parseDouble(values[0]);

        }else if(name.equals("Hang.Offset")){
            hangOffset = Double.parseDouble(values[0]);

        }else if (name.equals("Manual.Speed.Gain")) {
            manualSpeedGain = Double.parseDouble(values[0]);
            // the gain can't be less than 1, or the tolerance won't be exceeded
            if (manualSpeedGain > 1.0) {
                manualSpeed  = manualSpeedGain * ((maximumInput - minimumInput) * tolerance) / 100;
            }

        }else if (name.equals("Time.Limit")) {
            wristTimeLimit = Double.parseDouble(values[0]);
        }else if(name.equals("DeadZone")){
            wristDeadZone = Double.parseDouble(values[0]);
        }
    }

}